#include "LidarDriverRS.h"

#include <chrono>
#include <thread>

#pragma region Init
LidarDriverRS::LidarDriverRS(LidarConfigurationOptions *options, int id)
    : LidarDriverX(options, id)
{
    _driver.regExceptionCallback(std::bind(&LidarDriverRS::exceptionCallback, this, std::placeholders::_1));
    _driver.regPointCloudCallback(std::bind(&LidarDriverRS::pointCloudGetCallback, this), std::bind(&LidarDriverRS::pointCloudPutCallback, this, std::placeholders::_1));
}

void LidarDriverRS::init()
{
    // src
    _rsDriverParam.input_param.msop_port = std::get<uint16_t>(_options->GetValue(LidarConfigurationOptions::Data_SrcNet_MsopPort));
    _rsDriverParam.input_param.difop_port = std::get<uint16_t>(_options->GetValue(LidarConfigurationOptions::Data_SrcNet_DifopPort));
    LidarInputSourceType sourceType = std::get<LidarInputSourceType>(_options->GetValue(LidarConfigurationOptions::Data_Source_SrcType));
    switch (sourceType)
    {
    case LidarInputSourceType::ONLINE_LIDAR:
        _rsDriverParam.input_type = InputType::ONLINE_LIDAR;
        break;
    case LidarInputSourceType::PCAP_FILE:
        _rsDriverParam.input_type = InputType::PCAP_FILE;
        break;
    case LidarInputSourceType::RAW_PACKET:
        _rsDriverParam.input_type = InputType::RAW_PACKET;
        break;
    default:
        break;
    }
    LidarManufacturerType manuType = std::get<LidarManufacturerType>(_options->GetValue(LidarConfigurationOptions::Data_SrcNet_ManuType));
    switch (manuType)
    {
    case LidarManufacturerType::RS_16:
        _rsDriverParam.lidar_type = LidarType::RS16;
        break;
    case LidarManufacturerType::RS_32:
        _rsDriverParam.lidar_type = LidarType::RS32;
        break;
    case LidarManufacturerType::RS_BP:
        _rsDriverParam.lidar_type = LidarType::RSBP;
        break;
    case LidarManufacturerType::RS_HELIOS:
        _rsDriverParam.lidar_type = LidarType::RSHELIOS;
        break;
    case LidarManufacturerType::RS_128:
        _rsDriverParam.lidar_type = LidarType::RS128;
        break;
    case LidarManufacturerType::RS_80:
        _rsDriverParam.lidar_type = LidarType::RS80;
        break;
    case LidarManufacturerType::RS_M1:
        _rsDriverParam.lidar_type = LidarType::RSM1;
        break;
    default:
        break;
    }
    // TransformParam transform = std::get<TransformParam>(_options->GetValue(LidarConfigurationOptions::Data_Decode_Transform));
    // _rsDriverParam.decoder_param.transform_param.x = transform.x;
    // _rsDriverParam.decoder_param.transform_param.y = transform.y;
    // _rsDriverParam.decoder_param.transform_param.z = transform.z;
    // _rsDriverParam.decoder_param.transform_param.roll = transform.roll;
    // _rsDriverParam.decoder_param.transform_param.pitch = transform.pitch;
    // _rsDriverParam.decoder_param.transform_param.yaw = transform.yaw;

    // decode
    if (_decodeType != LidarDecodeType::RAW)
        _rsDriverParam.decoder_param.dense_points = true;
} 

void LidarDriverRS::dispose()
{
    _free_cloud_queue.clear();
    _stuffed_cloud_queue.clear();
}
#pragma endregion

#pragma region thread
void LidarDriverRS::processPointCloud()
{
    _rsDriverParam.print();
    if (!_driver.init(_rsDriverParam))
    {
        std::cout << "RS Driver init failed." << std::endl;
        _to_exit_process = true;
        return;
    }

    _driver.start();

    while (!_to_exit_process)
    {
        std::shared_ptr<PointCloudMsg> msg = _stuffed_cloud_queue.popWait();
        if (msg.get() == NULL)
            continue;

        pcl::PointCloud<PointXYZI>::Ptr pcl_pointcloud(new pcl::PointCloud<PointXYZI>);
        pcl_pointcloud->points.swap(msg->points);
        pcl_pointcloud->height = msg->height;
        pcl_pointcloud->width = msg->width;
        pcl_pointcloud->is_dense = msg->is_dense;

        PointCloudData<PointXYZI> data;
        decodePointCloud(data, pcl_pointcloud);

        data.sequence = msg->seq;
        data.frame_id = msg->frame_id;
        data.lidar_id = _lidar_id;

        _free_cloud_queue.push(msg);

        for (PointCloudDataCallback handler : _pointCloudDataCallback)
        {
            handler(data);
        }
    }
}
#pragma endregion

#pragma region Callback
void LidarDriverRS::exceptionCallback(const Error &code)
{
    std::cout << "exceptionCallback: " << code.toString() << std::endl;
}

std::shared_ptr<PointCloudMsg> LidarDriverRS::pointCloudGetCallback()
{
    std::shared_ptr<PointCloudMsg> msg = _free_cloud_queue.pop();
    if (msg.get() != NULL)
        return msg;

    return std::make_shared<PointCloudMsg>();
}

void LidarDriverRS::pointCloudPutCallback(std::shared_ptr<PointCloudMsg> msg)
{
    auto len = _stuffed_cloud_queue.push(msg);
    if (len > max_buffer_num)
        _stuffed_cloud_queue.pop();
}
#pragma endregion